import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.nxt.MotorPort;
import lejos.nxt.NXTMotor;
import lejos.nxt.SensorPort;

public class Sejway {

	static final int KP = 180;
	static final int KI = 6;
	static final int KD = 1286;
	static final int SCALE = 18;

	static NXTMotor mt1;
	static NXTMotor mt2;

	int offset;
	int prev_error;
	float int_error;

	LightSensor ls;

	public Sejway() {
		ls = new LightSensor(SensorPort.S2, true);
		mt1 = new NXTMotor(MotorPort.B);
		mt2 = new NXTMotor(MotorPort.C);
		// Motor.B.regulateSpeed(false);
		// Motor.C.regulateSpeed(false);
	}

	public void getBalancePos() {
		do {
			offset = ls.readNormalizedValue();
			LCD.drawInt(offset, 0, 2);
			LCD.clear();
			LCD.refresh();

		}while (!Button.ENTER.isPressed());
	}

	public void pidControl() {

		while (!Button.ESCAPE.isPressed()) {
			int normVal = ls.readNormalizedValue();

			int error = normVal - offset;

			if (error < 0)
				error = (int) (error * 1.8F);
			int_error = ((int_error + error) * 2) / 3;
			int deriv_error = error - prev_error;
			prev_error = error;

			int pid_val = (int) (KP * error + KI * int_error + KD * deriv_error)
					/ SCALE;

			LCD.drawInt(pid_val, 0, 2);

			if (pid_val > 100) {
				pid_val = 100;
			}
			if (pid_val < -100) {
				pid_val = -100;
			}

			int power = Math.abs(pid_val);
			power = 55 + (power * 45) / 100;
			mt1.setPower(power);
			mt2.setPower(power);
			// Motor.B.setPower(power);
			// Motor.C.setPower(power);

			if (pid_val > 0) {
				mt1.forward();
				mt2.forward();
				// Motor.B.forward();
				// Motor.C.forward();
			} else {
				mt1.backward();
				mt2.backward();
				// Motor.B.backward();
				// Motor.C.backward();
			}
		}
	}

	public void shutDown() {

		mt1.flt();
		mt2.flt();
		// Motor.B.flt();
		// Motor.C.flt();
		ls.setFloodlight(false);

	}

	public static void main(String[] args) {
		Sejway sej = new Sejway();
		sej.getBalancePos();
		sej.pidControl();
		sej.shutDown();
	}
}